#include "calibration/sensor_cali.h"
#include "kalman_filter/kalman.h"

static float magState[6] = {0.0f};
static float magP[36] = {
	100.0f,	0.0f,	0.0f,	0.0f,	0.0f,	0.0f,
	0.0f,	100.0f,	0.0f,	0.0f,	0.0f,	0.0f,
	0.0f,	0.0f,	100.0f, 0.0f,	0.0f,	0.0f,
	0.0f,	0.0f,	0.0f,	100.0f,	0.0f,	0.0f,
	0.0f,	0.0f,	0.0f,	0.0f,	100.0f,	0.0f,
	0.0f,	0.0f,	0.0f,	0.0f,	0.0f,	100.0f
};
static float magH[6] = {0.0f};
static float magR = 10.0f;

void mag_cali_measure(void *vfilter, float *estimate_output);
void mag_cali_computeH(void *vfilter);
void mag_cali_computeR(void *vfilter);

static struct kalman_filter mag_cali_kf = {
	.state_dim = 6,
	.measure_dim = 1,
	.x = magState,
	.P = magP,
	.A = NULL,
	.H = magH,
	.Q = NULL,
	.R = &magR,
	.process = NULL,
	.measurement = mag_cali_measure,
	.computeA = NULL,
	.computeQ = NULL,
	.computeH = mag_cali_computeH,
	.computeR = mag_cali_computeR
};

static struct mag_cali_monitor {
	float last_state[6];
	uint8_t converge_cnt;

	float prev_sel_mag[3];
	uint8_t too_close_cnt;

	uint8_t reset_filter;
	uint8_t status;
} mag_cali_monitor = {
	.status = CALI_DONE
};

static uint8_t check_mag_healthy(int16_t *raw, struct ellipsoid_param *res);

uint8_t mag_check_calibration(int16_t *raw,
			      struct ellipsoid_param *res,
			      uint8_t do_cali)
{
	/* variables for convergency check */
	uint8_t i;
	float variance[6] = {0.0};

	float fMag[3] = { raw[0], raw[1], raw[2] };
	/* for Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1 */
	float measure = 1.0;

	if (mag_cali_monitor.status != CALI_PROCESSING) {
		if (0 == do_cali) {
			return check_mag_healthy(raw, res);
		}
		mag_cali_monitor.status = CALI_PROCESSING;
		mag_cali_monitor.reset_filter = 1;
	}

	if (0 != mag_cali_monitor.reset_filter) {
		mag_cali_monitor.reset_filter = 0;
		/* reset mag calibration monitor */
		mag_cali_monitor.converge_cnt = 0;
		mag_cali_monitor.too_close_cnt = 0;
		mag_cali_monitor.last_state[0] = 0.0f;
		mag_cali_monitor.last_state[1] = 0.0f;
		mag_cali_monitor.last_state[2] = 0.0f;
		mag_cali_monitor.last_state[3] = 0.0f;
		mag_cali_monitor.last_state[4] = 0.0f;
		mag_cali_monitor.last_state[5] = 0.0f;
		/* reset filter */
		mag_cali_kf.x[0] = 1.0f;
		mag_cali_kf.x[1] = 1.0f;
		mag_cali_kf.x[2] = 1.0f;
		mag_cali_kf.x[3] = 1.0f;
		mag_cali_kf.x[4] = 1.0f;
		mag_cali_kf.x[5] = 1.0f;

		mag_cali_kf.P[0] = 10.0f;
		mag_cali_kf.P[7] = 10.0f;
		mag_cali_kf.P[14] = 10.0f;
		mag_cali_kf.P[21] = 10.0f;
		mag_cali_kf.P[28] = 10.0f;
		mag_cali_kf.P[35] = 10.0f;
	}

	if (vec3_norm(fMag[0] - mag_cali_monitor.prev_sel_mag[0],
		fMag[1] - mag_cali_monitor.prev_sel_mag[1],
		fMag[2] - mag_cali_monitor.prev_sel_mag[2])
		/ vec3_norm(fMag[0], fMag[1], fMag[2]) < 0.2) {
		if (mag_cali_monitor.too_close_cnt > 5)
			return CALI_PROCESSING;
		mag_cali_monitor.too_close_cnt++;
	} else {
		mag_cali_monitor.too_close_cnt = 0;
		mag_cali_monitor.prev_sel_mag[0] = fMag[0];
		mag_cali_monitor.prev_sel_mag[1] = fMag[1];
		mag_cali_monitor.prev_sel_mag[2] = fMag[2];
	}

	mag_cali_kf.H[0] = fMag[0] * fMag[0];
	mag_cali_kf.H[1] = fMag[1] * fMag[1];
	mag_cali_kf.H[2] = fMag[2] * fMag[2];
	mag_cali_kf.H[3] = 2 * fMag[0];
	mag_cali_kf.H[4] = 2 * fMag[1];
	mag_cali_kf.H[5] = 2 * fMag[2];

	kf_update(&mag_cali_kf, &measure);

	for (i = 0; i < 6; i++) {
		if (mag_cali_kf.x[i] < 1e-7 && mag_cali_kf.x[i] > -1e-7) {
			variance[i] = 1.0f;	/* avoid divide by zero */
		} else {
			variance[i] = (mag_cali_kf.x[i]
					- mag_cali_monitor.last_state[i])
					/ mag_cali_kf.x[i];
		}
		if (variance[i] < 0.0f) {
			variance[i] = -variance[i];
		}
		if (variance[i] > 0.05) { /* choosen by experience */
			mag_cali_monitor.converge_cnt = 0;
			mag_cali_monitor.last_state[0] = mag_cali_kf.x[0];
			mag_cali_monitor.last_state[1] = mag_cali_kf.x[1];
			mag_cali_monitor.last_state[2] = mag_cali_kf.x[2];
			mag_cali_monitor.last_state[3] = mag_cali_kf.x[3];
			mag_cali_monitor.last_state[4] = mag_cali_kf.x[4];
			mag_cali_monitor.last_state[5] = mag_cali_kf.x[5];
			return CALI_PROCESSING;
		}
	}
	/* mag calibration end */
	if (mag_cali_monitor.converge_cnt++ > 100) {
		state_to_param(mag_cali_kf.x, res);
		mag_cali_monitor.status = CALI_DONE;
		return CALI_DONE;
	}
	return CALI_PROCESSING;
}

void mag_compensation(int16_t* raw, struct ellipsoid_param* cali)
{
	raw[0] = (int16_t)((raw[0] - cali->offset[0]) / cali->scale[0]) * 500;
	raw[1] = (int16_t)((raw[1] - cali->offset[1]) / cali->scale[1]) * 500;
	raw[2] = (int16_t)((raw[2] - cali->offset[2]) / cali->scale[2]) * 500;
}

void mag_cali_measure(void *vfilter, float *estimateOut)
{
	struct kalman_filter *filter = (struct kalman_filter *)vfilter;
	*estimateOut = filter->x[0] * filter->H[0]
			+ filter->x[1] * filter->H[1]
			+ filter->x[2] * filter->H[2]
			+ filter->x[3] * filter->H[3]
			+ filter->x[4] * filter->H[4]
			+ filter->x[5] * filter->H[5];
}

void mag_cali_computeH(void *vfilter)
{
	return;
}

void mag_cali_computeR(void *vfilter)
{
	return;
}

static uint8_t check_mag_healthy(int16_t *raw, struct ellipsoid_param *res)
{
	float radx, rady, radz;
	static float deviation = 0.0f;

	if (res->scale[0] > -1e-7 && res->scale[0] < 1e-7)
		return CALI_REQUIRE;
	if (res->scale[1] > -1e-7 && res->scale[1] < 1e-7)
		return CALI_REQUIRE;
	if (res->scale[2] > -1e-7 && res->scale[2] < 1e-7)
		return CALI_REQUIRE;

	radx = (raw[0] - res->offset[0])/res->scale[0];
	rady = (raw[1] - res->offset[1])/res->scale[1];
	radz = (raw[2] - res->offset[2])/res->scale[2];

	deviation = 0.95f * deviation
		+ 0.05 * (1.0f - radx * radx - rady * rady - radz * radz);

	if (deviation > 0.2 || deviation < -0.2)
		return CALI_REQUIRE;
	return CALI_DONE;
}

